#include "MT6701CT_STD.h"
#include "../motor/Motor.h"
#include <cmath>
#include "../motor/Motor.h"
// 定义静态实例指针（必须且只能定义一次）
MT6701Sensor* MT6701Sensor::instance = nullptr;
extern TIM_HandleTypeDef htim1;

MT6701Sensor * MT6701Sensor::getInstance() {
    if (instance == nullptr) {
        instance = new MT6701Sensor(&hspi1, MT6701_CS_GPIO_Port, MT6701_CS_Pin, 7);
    }
    return instance; // 添加返回语句
}

MT6701Sensor::MT6701Sensor(SPI_HandleTypeDef* spi, GPIO_TypeDef* csPort, uint16_t csPin, uint8_t polePairs)
    : _spi(spi), _csPort(csPort), _csPin(csPin), _polePairs(polePairs) {}

void MT6701Sensor::csEnable() {
    HAL_GPIO_WritePin(_csPort, _csPin, GPIO_PIN_RESET);
}

void MT6701Sensor::csDisable() {
    HAL_GPIO_WritePin(_csPort, _csPin, GPIO_PIN_SET);
}

uint16_t MT6701Sensor::getRawData() {
    uint8_t txData[2] = { 0xFF, 0xFF };
    uint8_t rxData[2] = { 0 };
    uint16_t timeout = 200;

    while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {
        if (timeout-- == 0) {
            return 0;
        }
    }

    csEnable();
    HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(_spi, txData, rxData, 2, HAL_MAX_DELAY);
    csDisable();

    if (status != HAL_OK) {
        return 0;
    }

    // 注意顺序：MT6701 发的是高字节在前（Big Endian）
    uint16_t rawData = (rxData[0] << 8) | rxData[1];

    return rawData >> 2; // 高14位有效
}


float MT6701Sensor::getRawAngle() {
    uint16_t raw = getRawData() +  alignment_offset; ;
    return static_cast<float>(raw) / 16384.0f * _2PI;
}

float MT6701Sensor::getFullAngle() {
    float angle = getRawAngle();
    float deltaAngle = angle - _angleLast;

    if (fabsf(deltaAngle) > (0.8f * _2PI)) {
        _rotationCount += (deltaAngle > 0) ? -1 : 1;
        _rotationCountLast = _rotationCount;
    }

    _angleLast = angle;
    float ang= _rotationCount * _2PI + _angleLast;
    this->sensorData.angle=ang;
    return ang;
}

float MT6701Sensor::getVelocity() {
    float fullAngle = getFullAngle();
    float delta = (_rotationCount - _rotationCountLast) * _2PI + (fullAngle - _fullAngleLast);

    // 更新上一次的旋转计数和角度
    _fullAngleLast = fullAngle;
    _rotationCountLast = _rotationCount;

    // 10ms中断一次，所以乘以100.0f (1/0.01秒)
    float vel_rad_per_sec = delta * 100.0f; // 单位: rad/s

    // 转换为转/秒(RPS): rad/s ÷ (2π)
    float vel_rps = vel_rad_per_sec / _2PI;

    this->sensorData.velocity = vel_rps;
    return vel_rps;
}

float MT6701Sensor::getElectricalAngle() {
    return _normalizeAngle(_polePairs * getRawAngle() - _zeroElectricalOffset);
}

void MT6701Sensor::alignSensor() {
    // 强制进入STEP_1，锁定转子到A+ B-位置
    // 给固定的一步，强制拉住转子
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 500); // A+
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 500); // B-
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);              // C关

    // 延时一段时间，等待转子被吸稳
    HAL_Delay(1000);  // 1000ms，对准时间可以根据电机惯量调整

    currentStep = CommutationStep::STEP_1;
}


/**
 * 获取当前的六步换相区域（0-5）
 * @return 换相区域索引（对应BLDCMotor的CommutationStep）
 */
/**
 * 获取当前的六步换相区域（1-6）
 * @return 换相区域索引（1-6）
 */
uint8_t MT6701Sensor::getCommutationStep() {
    float electricalAngle = getElectricalAngle();

    // 将角度归一化到[0, 2PI)
    electricalAngle = fmod(electricalAngle, _2PI);
    if (electricalAngle < 0) electricalAngle += _2PI;

    // 将电角度映射到6个扇区（每扇区60度）
    // 计算结果范围：[0, 6) → 对应扇区 0-5
    float sectorAngle = electricalAngle * 3.0f / _PI;
    uint8_t sector = static_cast<uint8_t>(sectorAngle) % 6;

    // 转换为1-6编号（与BLDCMotor的CommutationStep枚举对应）
    uint8_t step= sector ;
    this->sensorData.step=step;
    return step;
}

uint8_t MT6701Sensor::getFilteredStep() {
    static uint8_t lastStep = 1;
    uint8_t currentStep = getCommutationStep();

    // 计算环形差值（例如：6和1的差值为1，而非5）
    uint8_t delta = (currentStep > lastStep) ?
                    (currentStep - lastStep) :
                    (lastStep - currentStep);
    delta = (delta > 3) ? (6 - delta) : delta; // 环形空间中，差值≤3视为相邻

    if (delta <= 1) { // 允许相邻扇区跳变（包括环形相邻，如6→1）
        if (currentStep == lastStep) {
            return currentStep; // 连续相同，确认有效
        } else {
            // 首次检测到相邻跳变，暂不更新，等待下次确认
            return lastStep;
        }
    } else {
        // 非相邻跳变（如2→4，delta=2>1），忽略本次数据
        return lastStep; // 保留上一次有效值
    }
}

// void MT6701Sensor::alignSensor() {
//     // 手动转动电机至已知机械零点
//     // 假设此时电角度为0°（对应扇区1中心）
//
// }